#include "constraint.h"
#include "frame.h"
#include "camera.h"
#include "manipulatedCameraFrame.h"


using namespace std;

////////////////////////////////////////////////////////////////////////////////
//                                  Constraint                                //
////////////////////////////////////////////////////////////////////////////////

/*! Default constructor.

translationConstraintType() and rotationConstraintType() are set to AxisPlaneConstraint::FREE.
translationConstraintDirection() and rotationConstraintDirection() are set to (0,0,0). */
AxisPlaneConstraint::AxisPlaneConstraint()
	: translationConstraintType_(FREE), rotationConstraintType_(FREE)
{
	// Do not use set since setRotationConstraintType needs a read.
}

/*! Simply calls setTranslationConstraintType() and setTranslationConstraintDirection(). */
void AxisPlaneConstraint::setTranslationConstraint(Type type, const Vec& direction)
{
	setTranslationConstraintType(type);
	setTranslationConstraintDirection(direction);
}

/*! Defines the translationConstraintDirection(). The coordinate system where \p direction is expressed depends on your class implementation. */
void AxisPlaneConstraint::setTranslationConstraintDirection(const Vec& direction)
{
	if ((translationConstraintType()!=AxisPlaneConstraint::FREE) && (translationConstraintType()!=AxisPlaneConstraint::FORBIDDEN))
	{
		const qreal norm = direction.norm();
		if (norm < 1E-8)
		{
			qWarning("AxisPlaneConstraint::setTranslationConstraintDir: null vector for translation constraint");
			translationConstraintType_ = AxisPlaneConstraint::FREE;
		}
		else
			translationConstraintDir_ = direction/norm;
	}
}

/*! Simply calls setRotationConstraintType() and setRotationConstraintDirection(). */
void AxisPlaneConstraint::setRotationConstraint(Type type, const Vec& direction)
{
	setRotationConstraintType(type);
	setRotationConstraintDirection(direction);
}

/*! Defines the rotationConstraintDirection(). The coordinate system where \p direction is expressed depends on your class implementation. */
void AxisPlaneConstraint::setRotationConstraintDirection(const Vec& direction)
{
	if ((rotationConstraintType()!=AxisPlaneConstraint::FREE) && (rotationConstraintType()!=AxisPlaneConstraint::FORBIDDEN))
	{
		const qreal norm = direction.norm();
		if (norm < 1E-8)
		{
			qWarning("AxisPlaneConstraint::setRotationConstraintDir: null vector for rotation constraint");
			rotationConstraintType_ = AxisPlaneConstraint::FREE;
		}
		else
			rotationConstraintDir_ = direction/norm;
	}
}

/*! Set the Type() of the rotationConstraintType(). Default is AxisPlaneConstraint::FREE.

 Depending on this value, the Frame will freely rotate (AxisPlaneConstraint::FREE), will only be able
 to rotate around an axis (AxisPlaneConstraint::AXIS), or will not able to rotate at all
 (AxisPlaneConstraint::FORBIDDEN).

 Use Frame::setOrientation() to define the orientation of the constrained Frame before it gets
 constrained.

 \attention An AxisPlaneConstraint::PLANE Type() is not meaningful for rotational constraints and
 will be ignored. */
void AxisPlaneConstraint::setRotationConstraintType(Type type)
{
	if (rotationConstraintType() == AxisPlaneConstraint::PLANE)
	{
		qWarning("AxisPlaneConstraint::setRotationConstraintType: the PLANE type cannot be used for a rotation constraints");
		return;
	}

	rotationConstraintType_ = type;
}


////////////////////////////////////////////////////////////////////////////////
//                               LocalConstraint                              //
////////////////////////////////////////////////////////////////////////////////

/*! Depending on translationConstraintType(), constrain \p translation to be along an axis or
  limited to a plane defined in the Frame local coordinate system by
  translationConstraintDirection(). */
void LocalConstraint::constrainTranslation(Vec& translation, Frame* const frame)
{
	Vec proj;
	switch (translationConstraintType())
	{
	case AxisPlaneConstraint::FREE:
		break;
	case AxisPlaneConstraint::PLANE:
		proj = frame->rotation().rotate(translationConstraintDirection());
		translation.projectOnPlane(proj);
		break;
	case AxisPlaneConstraint::AXIS:
		proj = frame->rotation().rotate(translationConstraintDirection());
		translation.projectOnAxis(proj);
		break;
	case AxisPlaneConstraint::FORBIDDEN:
		translation = Vec(0.0, 0.0, 0.0);
		break;
	}
}

/*! When rotationConstraintType() is AxisPlaneConstraint::AXIS, constrain \p rotation to be a rotation
  around an axis whose direction is defined in the Frame local coordinate system by
  rotationConstraintDirection(). */
void LocalConstraint::constrainRotation(Quaternion& rotation, Frame* const)
{
	switch (rotationConstraintType())
	{
	case AxisPlaneConstraint::FREE:
		break;
	case AxisPlaneConstraint::PLANE:
		break;
	case AxisPlaneConstraint::AXIS:
	{
		Vec axis = rotationConstraintDirection();
		Vec quat = Vec(rotation[0], rotation[1], rotation[2]);
		quat.projectOnAxis(axis);
		rotation = Quaternion(quat, 2.0*acos(rotation[3]));
	}
		break;
	case AxisPlaneConstraint::FORBIDDEN:
		rotation = Quaternion(); // identity
		break;
	}
}

////////////////////////////////////////////////////////////////////////////////
//                               WorldConstraint                              //
////////////////////////////////////////////////////////////////////////////////

/*! Depending on translationConstraintType(), constrain \p translation to be along an axis or
  limited to a plane defined in the world coordinate system by
  translationConstraintDirection(). */
void WorldConstraint::constrainTranslation(Vec& translation, Frame* const frame)
{
	Vec proj;
	switch (translationConstraintType())
	{
	case AxisPlaneConstraint::FREE:
		break;
	case AxisPlaneConstraint::PLANE:
		if (frame->referenceFrame())
		{
			proj = frame->referenceFrame()->transformOf(translationConstraintDirection());
			translation.projectOnPlane(proj);
		}
		else
			translation.projectOnPlane(translationConstraintDirection());
		break;
	case AxisPlaneConstraint::AXIS:
		if (frame->referenceFrame())
		{
			proj = frame->referenceFrame()->transformOf(translationConstraintDirection());
			translation.projectOnAxis(proj);
		}
		else
			translation.projectOnAxis(translationConstraintDirection());
		break;
	case AxisPlaneConstraint::FORBIDDEN:
		translation = Vec(0.0, 0.0, 0.0);
		break;
	}
}

/*! When rotationConstraintType() is AxisPlaneConstraint::AXIS, constrain \p rotation to be a rotation
  around an axis whose direction is defined in the world coordinate system by
  rotationConstraintDirection(). */
void WorldConstraint::constrainRotation(Quaternion& rotation, Frame* const frame)
{
	switch (rotationConstraintType())
	{
	case AxisPlaneConstraint::FREE:
		break;
	case AxisPlaneConstraint::PLANE:
		break;
	case AxisPlaneConstraint::AXIS:
	{
		Vec quat(rotation[0], rotation[1], rotation[2]);
		Vec axis = frame->transformOf(rotationConstraintDirection());
		quat.projectOnAxis(axis);
		rotation = Quaternion(quat, 2.0*acos(rotation[3]));
		break;
	}
	case AxisPlaneConstraint::FORBIDDEN:
		rotation = Quaternion(); // identity
		break;
	}
}

////////////////////////////////////////////////////////////////////////////////
//                               CameraConstraint                              //
////////////////////////////////////////////////////////////////////////////////

/*! Creates a CameraConstraint, whose constrained directions are defined in the \p camera coordinate
  system. */
CameraConstraint::CameraConstraint(const Camera* const camera)
	: AxisPlaneConstraint(), camera_(camera)
{}

/*! Depending on translationConstraintType(), constrain \p translation to be along an axis or
  limited to a plane defined in the camera() coordinate system by
  translationConstraintDirection(). */
void CameraConstraint::constrainTranslation(Vec& translation, Frame* const frame)
{
	Vec proj;
	switch (translationConstraintType())
	{
	case AxisPlaneConstraint::FREE:
		break;
	case AxisPlaneConstraint::PLANE:
		proj = camera()->frame()->inverseTransformOf(translationConstraintDirection());
		if (frame->referenceFrame())
			proj = frame->referenceFrame()->transformOf(proj);
		translation.projectOnPlane(proj);
		break;
	case AxisPlaneConstraint::AXIS:
		proj = camera()->frame()->inverseTransformOf(translationConstraintDirection());
		if (frame->referenceFrame())
			proj = frame->referenceFrame()->transformOf(proj);
		translation.projectOnAxis(proj);
		break;
	case AxisPlaneConstraint::FORBIDDEN:
		translation = Vec(0.0, 0.0, 0.0);
		break;
	}
}

/*! When rotationConstraintType() is AxisPlaneConstraint::AXIS, constrain \p rotation to be a rotation
  around an axis whose direction is defined in the camera() coordinate system by
  rotationConstraintDirection(). */
void CameraConstraint::constrainRotation(Quaternion& rotation, Frame* const frame)
{
	switch (rotationConstraintType())
	{
	case AxisPlaneConstraint::FREE:
		break;
	case AxisPlaneConstraint::PLANE:
		break;
	case AxisPlaneConstraint::AXIS:
	{
		Vec axis = frame->transformOf(camera()->frame()->inverseTransformOf(rotationConstraintDirection()));
		Vec quat = Vec(rotation[0], rotation[1], rotation[2]);
		quat.projectOnAxis(axis);
		rotation = Quaternion(quat, 2.0*acos(rotation[3]));
	}
		break;
	case AxisPlaneConstraint::FORBIDDEN:
		rotation = Quaternion(); // identity
		break;
	}
}
